function [FINSSens2SysBlk] = naverrdynmat_inssens2sysblk(CBN, fB, omegaIBB, INSSysStateMask, INSSensStateMask, DTGgSensSymmetry) %#codegen
%NAVERRDYNMAT_INSSENS2SYSBLK 计算误差动态矩阵的INS器件状态→系统状态块
%
% Input Arguments
% # CBN: 3*3矩阵，姿态方向余弦矩阵，无单位
% # fB: 3*1向量，比力，单位m/s^2
% # omegaIBB: 3*1向量，角速度，单位rad/s
% # INSSysStateMask: 1*11逻辑向量，true表示包含该系统状态，false表示不包含该系统状态
% # INSSensStateMask: 1*36逻辑向量，true表示包含该器件状态，false表示不包含该器件状态
% # DTGgSensSymmetry: 1*9的int8类型向量，用于指示挠性惯组g敏感项误差中的对称性，在mask指示包含的g敏感项误差中，DTGgSensSymmetry元素与包含参数相同的为该包含参数的正对称参数，相反的为该包含参数的负对称参数，0表示不存在对称参数，仅考虑第一个对称参数
%
% Output Arguments
% # FINSSens2SysBlk: 误差动态矩阵的INS器件状态→系统状态块
%
% Assumptions and Limitations
% # 本函数假设完整的系统状态中速度误差为第4-6个元素，姿态误差为第7-9个元素
% # 理论上，传入本函数的导航参数应为理想值，实际计算中以惯导计算的参数为输入
%
% References
% # 《应用导航算法工程基础》“考虑惯性器件误差模型的线性化方程组”

coder.extrinsic('warning', 'sym');
if length(INSSensStateMask) ~= 36
    warning('kfdynmatinssens2sysblk:INSSensStateMask', 'INS sensor state mask length invalid, must be 36.');
end

if isempty(coder.target) && (isa(CBN, 'sym') || isa(fB, 'sym') || isa(omegaIBB, 'sym'))
    FINSSens2SysBlk = zeros(nnz(INSSysStateMask), nnz(INSSensStateMask), 'sym');
else
    FINSSens2SysBlk = zeros(nnz(INSSysStateMask), nnz(INSSensStateMask));
end
% 加速度计相关
F12PrimeAcc = naverrdynmat_inssenstriad2sysblk(CBN, fB, [], true, INSSensStateMask(:, 1:15));
beginRowNum = nnz(INSSysStateMask(1:3));
beginColumnNum = 0;
subSysStateMask = INSSysStateMask(4:6);
endRowNum = beginRowNum + nnz(subSysStateMask);
endColumnNum = beginColumnNum + nnz(INSSensStateMask(:, 1:15));
FINSSens2SysBlk(beginRowNum+1:endRowNum, beginColumnNum+1:endColumnNum) = F12PrimeAcc(subSysStateMask, INSSensStateMask(:, 1:15));
% 陀螺相关
F12PrimeGyro = naverrdynmat_inssenstriad2sysblk(CBN, omegaIBB, fB, false, INSSensStateMask(:, 16:36));
% 挠性惯组对称性处理
for i=1:9
    if INSSensStateMask(1, 27+i) && (DTGgSensSymmetry(1, i)~=0)
        for j=1:9
            if j ~= i
                if DTGgSensSymmetry(1, j) == DTGgSensSymmetry(1, i)
                    F12PrimeGyro(1:3, 12+i) = F12PrimeGyro(1:3, 12+i) + F12PrimeGyro(1:3, 12+j);
                    break;
                elseif DTGgSensSymmetry(1, j) == -DTGgSensSymmetry(1, i)
                    F12PrimeGyro(1:3, 12+i) = F12PrimeGyro(1:3, 12+i) - F12PrimeGyro(1:3, 12+j);
                    break;
                end
            end
        end
    end
end
beginRowNum = endRowNum;
beginColumnNum = endColumnNum;
subSysStateMask = INSSysStateMask(7:9);
endRowNum = beginRowNum + nnz(subSysStateMask);
endColumnNum = beginColumnNum + nnz(INSSensStateMask(:, 16:36));
FINSSens2SysBlk(beginRowNum+1:endRowNum, beginColumnNum+1:endColumnNum) = F12PrimeGyro(subSysStateMask, INSSensStateMask(:, 16:36));
end